#include "turret.h"
#include "../Commands/TurretJoy.h"
#include "../CommandBase.h"
#define SLOWER 1
#define NUM_TICKS 33

Turret::Turret() : Subsystem("turret")
{	
	turretJaguar = new CANJaguar(TURRET_JAGUAR, CANJaguar::kPosition);
	turretJaguar->SetPositionReference(CANJaguar::kPosRef_QuadEncoder);
	turretJaguar->SetPID(1000,0,0);
	turretJaguar->ConfigEncoderCodesPerRev(2048*TURRET_SPROCKET_TEETH/TURRET_MOTOR_TEETH);
	turretJaguar->EnableControl(0);
}

void Turret::turretJoystick(Joystick *joystick)
{
	turretJaguar->Set(joystick->GetX()/2.0f);
	Wait(.01);
	printf("%f\n",turretJaguar->GetPosition());
}

void Turret::InitDefaultCommand()
{
	SetDefaultCommand(new TurretJoy());
}
